#ifndef _BALANCE_CAL_H
#define _BALANCE_CAL_H

#include "M3508.h"
extern DJI_Motor_ControlTypeDef M3508_Motor;

/**
 * @brief 欧拉角
 * @brief Pitch(俯仰角)
 * @brief Yaw (航向角)
 * @brief Roll(翻滚角)
 */
typedef struct
{
	float Pitch;
	float Yaw;
	float Roll;
} Position;  // unit: m, rad

/**
 * @brief 速度信息
 * @param Vx x方向速度,m/s
 * @param Vy y方向速度,m/s
 * @param Vw 航向角速度,rad/s
 * @note Robot.Robot_in_self.velocity_exp. Robot为Robot_INFO变量
 */
typedef struct
{
	float Vx;
	float Vy;
	float Vw;
} Velocity; // unit: m/s, rad/s

/**
 * @brief 加速度信息
 * @param Ax x方向加速度,m/s^2
 * @param Ay y方向加速度,m/s^2
 * @param Aw 航向角加速度,rad/s^2
 */
typedef struct
{
	float Ax;
	float Ay;
	float Aw;
} Acceleration; // unit: m/s^2, rad/s^2

/**
 * @brief 机器人位置信息
 * @param position_now 当前位置信息
 * @param velocity_now 当前速度信息
 * @param position_exp 期望位置信息
 * @param velocity_exp 期望速度信息
 * @param position_last 上一次位置信息
 * @param velocity_last 上一次速度信息
 * @param acceleration_now 当前加速度信息
 *
 */
typedef struct
{
	Position position_now;
	Velocity velocity_now;
	Position position_exp;
	Velocity velocity_exp;
	Position position_last;
	Velocity velocity_last;
	Acceleration acceleration_now;
} Wheel_Pos;
/**
 * @brief 机器人信息
 * @param Robot_in_self 机器人在自身坐标系下的位置信息
 * @param Robot_in_world 机器人在世界坐标系下的位置信息
 * @param Robot_Contorl_Mode 机器人控制模式
 * @param Wheel_Speed 机器人轮子速度
 *
 */
typedef struct
{
	Wheel_Pos Robot_in_self;
	uint8_t Robot_Contorl_Mode;
	float Wheel_Speed[2];
	float Leg_Height[2];        //0 left 1 right
	float Robot_Speed_max;
} Wheel_Data;

extern Wheel_Data Balance;

void Robot_Velocity_To_Wheel_Speed(Wheel_Data* robot);
void World_To_Self(Wheel_Data* robot);
void Wheel_Speed_To_Motor(Wheel_Data* robot,DJI_Motor_ControlTypeDef* motor);

#endif
